Trajectory control by means of linear interpolation in the robot control is to provide controls so that a trajectory of a tip of a robot arm is a straight line with the velocity pattern like, for instance, a trapezoid.
FIG. 7 shows an intra-planar two-freedom SCARA type of robot. A freedom SCARA type of robot 20 has a first arm 24 which can rotate on a plane (X-Y plane) around a first joint shaft 21, and a second arm 25 which can rotate on a plane (X-Y plane) around a second joint shaft 22; wherein a tip (arm tip) 27 of the second arm 25 is a coordinate point as an object for robot control.
FIG. 8 shows a robot control apparatus using a microcomputer. The robot control apparatus comprises a central processing unit 1, an I/O interface 4 connecting an input device 2 and an output device 3 to the central processing unit 1, amemory5, a servo amplifier interface 6, and an encoder interface 7. The memory 5 includes a control program storing area 8, an arm tip position storing area 9, a joint angle storing area 10, an end-point position storing area 11, and an arm length storing area 12.
Connected to the servo amplifier interface 6 is a servo amplifier 31 for driving servo motors provided in each joint shaft for driving each arm of a robot 20, and a joint encoder 32 provided in each joint shaft for detecting an angle of each arm of the robot 20 is connected to the encoder interface 7.
Next description is made for a trajectory control method based in the conventional technology for a SCARA type of robot by means of linear interpolation with reference to FIG. 9. At first, the central processing unit 1 starts linear interpolation (step S101). The sequence for this linear interpolation is called out from an operating sequence such as planning of works for upper equipment. When this sequence is called out, a current position and a final target position are decided through an operating sequence in the upper equipment.
When linear interpolation is started, at first a positional increment for an arm tip is obtained from a start position and an end position of a path for linear interpolation and a specified velocity each stored in the memory 5 (step S102). This arm tip position increment is obtained by multiplying the specified velocity by a sampling time as a positional increment in a direction from the start point to end point of the arm tip position at each sampling time.
Then, an arm tip position to be reached at the next sampling time is obtained by adding the arm tip position increment obtained in step S102 to an arm tip position at a current point of time stored in the arm tip position storing area 9 provided in the memory 5 (step S103). Then, an angle of each joint at the next sampling time is obtained by executing reverse conversion for the joint-arm tip position from the arm tip position at the next sampling time obtained in step S103 (step S104).
Then, an angular velocity of joint to move at the current sampling time is obtained by subtracting a current joint angle stored in the joint angle storing area 10 provided in the memory 5 from the angle of each joint at the next sampling time obtained in step S104 (step S105). Then, the joint angle to be driven obtained in step S105 is given to a servo amplifier 31 through the servo interface 6 to drive servo motors provided in each joint of the robot 20 (step S106).
Then, a joint angle of each arm is detected by a joint encoder 32 provided in each joint of the robot 20 through the encoder interface 7 when the next sampling time has come, and the value of the joint angle each of current arm in the joint angle storing area 10 is updated (step S107). Then, a current arm tip position is obtained by executing regular conversion for a joint angle-an arm point position from the current joint angle (step S108).
Then, a value of the arm tip position written in the arm tip point storing area 9 provided in the memory 5 is updated by the current arm tip position obtained in step S108 (step S109). Then, the coordinates of the end position stored in the end position storing area 11 provided in the memory 5 are compared to those of the current arm tip position obtained in step S108 (step S10). If the current position has reached the end position, the processing is branched to End (step S111), while if not, the processing is branched to step S103 to execute again the subsequent steps thereto.
Next description is made for concrete operations of the two-freedom SCARA type of robot with reference to the explanatory view of operations shown in FIG. 10 as well as to the graphs shown in FIG. 11 and FIG. 12.
Herein, specifications of the two-freedom SCARA type of robot are set as follows:
Length of the first arm L.sub.1 : 320 mm PA1 Length of the second arm L.sub.2 : 320 mm PA1 First joint-maximum rated angular velocity: 4.12 rad/sec PA1 Second joint-maximum rated angular velocity: 4.12 rad/sec PA1 Sampling time: 28.4 msec
FIG. 10 shows a case where the robot is operated with linear interpolation from a point of 500 mm in the X coordinate and -100 mm in the Y coordinate (a start position) to a point of -500 mm in the X coordinate and -100 mm in the Y coordinate (an end position) in parallel to the X-axis. FIG. 10 shows a state (posture) of the robot arm at each 0.284 sect namely, or at each 10 sampling time when an arm tip position velocity is 390 mm/sec, namely a positional increment for the arm tip for each sampling time is 11 mm. Herein, a velocity is selected so that an operating speed of a joint is the highest within a range not exceeding the maximum rating.
As shown in FIG. 10, the first shaft (first joint shaft 21) at first moves in the counterclockwise direction from the start position (X coordinate: 500 mm; Y coordinate: -100 mm) to the arm state indicated by the reference character R, namely to the posture in which the second arm 25 and the direction of movement thereof form a right angle. The first shaft moves in the clockwise direction from this position to the end position (X coordinate: -500 mm; Y coordinate: -100 mm). The rotating direction of the first shaft is reversed at this R point (a point of an arm state). Namely, the point R is a rocking point of a slider crank mechanism comprising the first shaft, first arm 24, second shaft (second joint shaft 22), second arm 25, arm tip, and linear path.
FIG. 11 is a state diagram in which the X-axis indicates an X coordinate of an arm tip and the Y-axis indicates an positional increment for an arm tip at a sampling time, an angle of the first shaft, and an angular velocity of the first shaft. In FIG. 11, designated at the reference pattern of a black circle ".circle-solid." is an angle of the first shaft, at a white square ".quadrature." an angular velocity of the first shaft, and at a black triangle ".tangle-solidup." a positional increment for an arm tip at each sampling time in the tip of the shaft respectively.
FIG. 12 is a state diagram in which the X-axis indicates a sampling time and the Y-axis indicates, as shown in FIG. 11, a positional increment for an arm tip, an angle of the first shaft, and an angular velocity of the first shaft at the sampling time. In FIG. 12, designated at the reference pattern of a black circle ".circle-solid." is an angle of the first shaft, at a white square ".quadrature." an angular velocity of the first shaft, and at a black triangle ".tangle-solidup." a positional increment for an arm tip at each sampling time in the tip of the shaft respectively.
It is found from FIG. 11 that an angular velocity of the first shaft is a maximum at the point of time when the arm tip position has come close to the origin, namely at the point of time when the X coordinate has approached zero. At this point of time, an instructed velocity is restricted so that the angular velocity of the first shaft does not exceed the maximum rating. For this reason, the first shaft is used only by around a half of the capability at points other than those adjacent to the point of X=0 mm.
It is one of very important objects for controlling that a velocity of an arm tip is kept constant when the SCARA type of robot carries out works suited for CP control (Continuous Path control) such as painting work.
However, it is not always an indispensable condition that a trajectory of an arm tip is a straight line and also a velocity is kept constant when the SCARA type of robot is used in assembly operations of which main operation is to control picking and placing. It is rather strongly required that linear operation from a start position to an end position is executed for a short period of time to enhance efficiency of the operation. In this case, it is preferred to improve the operability by making the best use of capability of the first joint shaft 21 but not to keep constant a velocity of an arm tip of the SCARA type of robot.
From the above description, a linear interpolation control method is conceivable for operation at a high velocity with the first joint shaft 21 of which angular velocity is constant by making the best use of capability of a servo motor to drive the first joint shaft 21.
Description is made for a method of controlling linear interpolation in which an arm tip is operated on a straight line by keeping an angular velocity of the first joint shaft 21 to be constant with reference to FIG. 13.
Linear interpolation is started (step S101). The sequence for this linear interpolation is called out, for instance, from an operating sequence such as work plan for upper equipment. When this sequence is called out, a current position and a final target position are decided through an operating sequence in the upper equipment. An angular velocity of the first joint in the linear interpolating operation is given through an operating sequence in the upper equipment. When linear interpolation is started, a positional increment for an arm tip is obtained under the condition in which the angular velocity of the first joint is constant (step S112).
A relation between time differentiation for an arm tip position (X, Y) of the two-freedom SCARA type of robot shown in FIG. 7, namely a velocity of an arm tip position and time differentiation for a joint angle (.theta..sub.1, .theta..sub.2), namely an angular velocity of the joint is expressed by the expression (2). Wherein J indicates Jacobean matrix expressed by the expression (3). ##EQU1##
This reversed relation is expressed by the expression (4). ##EQU2##
Herein Jr indicates reversed Jacobean matrix expressed by the expression (5). ##EQU3##
The expression (4) is a binomial simultaneous equations. Through this first expression, a relation between the angular velocity of the first shaft and a velocity of an arm tip position is expressed by the expression (6). ##EQU4## wherein J.sub.1 indicates an angular increment for the first shaft at each sampling time, Vx indicates a positional increment for an arm tip in the X-axial direction, and Vy indicates a positional increment in the Y-axial direction. A relation between the angular increment J, and positional increments Vx, Vy of an arm tip is expressed by the expression (7). ##EQU5##
As shown in FIG. 14, it is assumed that P indicates a current position, E indicates an end position, and an arm tip is operated along the straight line PE. It is assumed that Vn indicates a positional increment for an arm tip in the current position P, Vx indicates an increment in the X-axial direction, and Vy indicates an increment in the Y-axial direction. It is also assumed that Dn indicates a remaining distance to be moved from a current position P to an end position E, Prx indicates an element of the remaining distance in the X direction, and Pry indicates an element of the remaining distance in the Y direction.
As shown in FIG. 14, there is the relation shown in the expression (8) between the remaining distance Dn, Prx and the increments to be moved Vn, Vx because a right triangle with the line segment PE as the hypotenuse and that with the vector Vn for the positional increment as the hypotenuse are similar to each other. ##EQU6##
Accordingly, a relation indicated by the expression (9) holds good. Similarly the expression (10) also holds good for the element in the Y-axial direction. ##EQU7##
If the expression (9) and the expression (10) are substituted into the expression (7), the expression (11) holds good. ##EQU8##
When the positional increment Vn of an arm tip in the expression (11) is solved, the expression (12) holds good. ##EQU9##
A positional increment Vn for an arm tip for controlling the arm tip is obtained in a linear trajectory under the condition of an angular velocity of the first shaft to be constant by substituting an instructed angular velocity of the joint into the angular increment J.sub.1 of the first shaft in the expression (12). By substituting a positional increment Vn for an arm tip into the expressions (9) and (10), an increment for positional coordinates for the arm tip is obtained.
As described above, when a positional increment has been obtained, the positional increment obtained as described above is added to the positional coordinates for the arm tip stored in the arm tip position storing area 9 provided in the memory 5, then an arm tip position to be reached at the next sampling time is obtained (step 5103).
Step S104 to step S111 in the flowchart shown in FIG. 13 is the same as that shown in FIG. 9 for controlling linear interpolation at a constant velocity of the arm tip, so that description thereof is omitted herein.
Next description is made for concrete operations of the SCARA type of robot with reference to an explanatory view of the operations shown in FIG. 15 and graphs shown in FIG. 16 and FIG. 17. It is assumed that the specifications of the SCARA type of robot are the same as those described above.
FIG. 15 shows, as a concrete example of the operations, a case where the robot is operated by linear interpolation from a position (a starting position) at X coordinate: 500 mm; Y coordinate: -100 mm to a position (an end position) at X coordinate: -500 mm; Y coordinate: -100 mm in parallel to the X-axis. FIG. 15 shows the robot arm for each 0.284 sec, namely for each 10 sampling time in a case where the angular velocity of the first shaft is 2.3 rad/sec. Herein, the highest velocity within a range in which the operating velocity of a shaft does not exceed the maximum rating is selected.
As shown in FIG. 15, the first shaft at first moves in the counterclockwise direction from the start position (X coordinate: 500 mm; Y coordinate: -100 mm) to the arm state indicated by the reference character R; namely to the posture in which the second arm 25 and the direction of movement thereof form a right angle. The first shaft moves in the clockwise direction from this state to the end position (X coordinate: -500 mm; Y coordinate: -100 mm). The rotating direction of the first shaft is reversed at this R point. Namely, the R point is a rocking point of a slider crank mechanism comprising the first shaft, first arm 24, second shaft, second arm 25, arm tip, and linear path.
FIG. 16 is a state diagram in which the X-axis indicates an X coordinate of an arm tip and the Y-axis indicates an positional increment for an arm tip at a sampling time, an angle of the first shaft, and an angular velocity of the first shaft. In FIG. 16, designated at the reference pattern of a black circle ".circle-solid." is an angle of the first shaft, at a white square ".quadrature." an angular velocity of the first shaft, and at a black triangle ".tangle-solidup." a positional increment for an arm tip at each sampling time in the arm tip of the shaft respectively.
FIG. 17 is a state diagram in which the X-axis indicates a sampling time and the Y-axis indicates, as shown in FIG.16, a positional increment for an arm tip, an angle of the first shaft, and an angular velocity of the first shaft at the sampling time. In FIG. 17, designated at the reference pattern of a black circle ".circle-solid." is an angle of the first shaft, at a white square ".quadrature." an angular velocity of the first shaft, and at a black triangle ".tangle-solidup." a positional increment for an arm tip at each sampling time in the arm tip of the shaft respectively.
An angular velocity of the first shaft indicated by the white square ".quadrature." is operated at substantially constant angular velocity at adjacent to +2.3 rad/sec as far as the angular velocity reaches the rocking point at 232 mm in the X coordinate shown in FIG. 16, and is operated at substantially constant angular velocity at adjacent to -2.3 rad/sec when the angular velocity exceeds the rocking point.
In linear interpolating operation at a constant velocity at the arm tip position, the arm tip is moved requiring 2.56 sec for a segment of 1000 mm from the start position X coordinate: 500 mm; Y coordinate: -100 mm) to the end position (X coordinate: -500 mm; Y coordinate: -100 mm), but when it is controlled at a constant angular velocity, only 1.48 sec is required for completion of the movement.
In the method of linear interpolation at a constant velocity of an arm tip based on the conventional technology, it is impossible to further reduce a tact time (operating time) by making use of capability of the driving motor for joints, but it is possible to further reduce a tact time (operating time) by making use of capability of the driving motor for a joint if the method of controlling linear interpolation at a constant angular velocity of a joint as described above is employed.
In the method of controlling linear interpolation at a constant angular velocity of a joint as described above, however, a positional increment for the arm tip at a sampling time is decided by the expression (12), so that, when the denominator is zero at the rocking point at which the rotating direction of the joint shaft is reversed, a positional increment Vn of the arm tip is disadvantageously diverged to infinity.
For this reason, the line indicating the positional increment for the arm tip at each sampling time in the arm tip of the shaft indicated by the black triangle ".tangle-solidup." in FIG. 16 is diverged to infinity at the point adjacent to the rocking point (X=232 mm) in this robot.
The angular velocity of the first shaft indicated by the white square ".quadrature." rotates, affected by this divergence, twice as fast as a specified angular velocity. Conversely, because of this point, a specified angular velocity has to be reduced so as not to generate excess of the velocity over an expression for a maximum rated rotation, which is inconvenient.